package com.googlecode.grs.core;

import com.googlecode.grs.environment.SimulationEnvironment;
import com.googlecode.grs.sensor.Servo;

/**
 * A MountedSimulatedSensor is a sensor that has a specific angular orientation
 * with respect to the robot. This orientation can be modified by a possibly
 * included servo.
 * 
 * @author ajc
 * 
 */
public abstract class MountedSimulatedSensor extends SimulatedSensor {

	private static final double DEFAULT_MOUNT_ANGLE = 0.;
	private static final Servo DEFAULT_SERVO = null;

	private double mountAngle;// TODO make final
	private Servo platform;

	public MountedSimulatedSensor(SimulationEnvironment env, Servo platform,
			double x, double y, double mountAngle) {
		super(env, x, y);
		this.mountAngle = mountAngle;
		this.platform = platform;
		// TODO Auto-generated constructor stub
	}

	/**
	 * ServoOffset describes the angle this sensor takes relative to the servo.
	 * Where RobotOffset is the angle relative to robot, this describes the
	 * permanent relationship between the sensor and servo.
	 * 
	 * It is also known as 'MountAngle' if using old terminology.
	 * 
	 * @return the sensor-servo offset angle, in radians (ccw: negative,cw
	 *         positive)
	 */
	public double getServoOffset() {
		return mountAngle;
	}

	/**
	 * RobotOffset is the difference of sensor heading and robot heading. It is
	 * the 'offset' of the sensor, the angle of the sensor relative to the
	 * robot. It doesn't change as robot heading changes. A MountAngle between
	 * sensor and servo
	 * 
	 * @return sensor-robot offset angle, in radians (ccw: negative,cw positive)
	 */
	public double getRobotOffset() {
		return platform.getAngle() + mountAngle;
	}

	/**
	 * Heading is the effective heading of the sensor. If servo angle and sensor
	 * mount angle were both 0, sensor heading would be equal to robot heading
	 * 
	 * @return effective heading
	 */
	public double getHeading() {
		return robotHeading() + platform.getAngle() + mountAngle;
	}

	/**
	 * Rotates the servo such that a sensor with 0 sensor-servo offset will have
	 * a sensor-robot offset of a provided angle
	 * 
	 * @param angle
	 *            difference from robot heading (negative ccw, positive cw) in
	 *            radians
	 */
	public void rotateServoTo(double angle) {
		platform.rotateTo(angle);
	}

	/**
	 * Rotates the servo such that the sensor-robot offset is equal to a
	 * provided angle
	 * 
	 * There is compensation for the sensor's default mount angle.
	 * 
	 * @param angle
	 *            difference from robot heading (negative ccw, positive cw) in
	 *            radians
	 */
	public void rotateTo(double angle) {
		platform.rotateTo(angle);
	}

	/**
	 * The servo is the controlled spinning platform on which this sensor sits.
	 * 
	 * @return sensor platform
	 */
	public Servo getServo() {
		return platform;
	}

}
